package com.rockwell.sniffyhunter.model;

import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;

import net.wimpi.modbus.ModbusException;

public class RobotPositionDetector implements IRobotPositionDetector {

	public static final int LeftCenterDistance = 5000;
	public static final int LeftTooCloseDistance = 10000;
	public static final int LeftCloseDistance = 7000;
	public static final int RightTooCloseDistance = 10000;
	public static final int RightCloseDistance = 7000;
	public static final int FrontTooCloseDistance = 14000;
	public static final int FrontCloseDistance = 12000;
	
	private IRobotSensors m_sensors;
	private boolean m_hasLeftSensor, m_hasRightSensor;
	private RobotPosition m_prevPosition = new RobotPosition(false, false, false);
	
	public RobotPositionDetector(IRobotSensors sensors) {
		m_sensors = sensors;
		m_hasLeftSensor = sensors.hasLeftDistanceSensor();
		m_hasRightSensor = sensors.hasRightDistanceSensor();
	}
	
	@Override
	public final RobotPosition getPosition() {
		if (m_hasLeftSensor && m_hasRightSensor) {
			return getPositionWithFLRSensors();
		}
		else if (m_hasLeftSensor) {
			return getPositionWithFLSensors();
		}
		else if (m_hasRightSensor) {
			return getPositionWithFRSensors();
		}
		
		// TODO Auto-generated method stub
		return null;
	}

	public final RobotPosition getPreviousPosition()
	{
		return m_prevPosition;
	}
	
	private final RobotPosition getPositionWithFLSensors() {
		return null;
	}

	
	private final RobotPosition getPositionWithFRSensors() {
		return null;
	}

	private final RobotPosition getPositionWithFLRSensors() {
		
		int flags = m_sensors.getSensorFlags();
		RobotPosition position = new RobotPosition(flags);
		
		boolean leftWallDetected = position.leftwalldetected && position.leftwall2detected;
		boolean rightWallDetected = position.rightwalldetected && position.rightwall2detected;
		
		// left and right wall within threshold (detected)
		if (leftWallDetected && rightWallDetected) {
			if (!position.frontwalldetected)
				position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.LaneCenter;
			else
				position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.DeadEnd;
		}
		else if (leftWallDetected && !rightWallDetected) {
			// right beyond threshold (not detected) and left wall within threshold (detected)
			if (position.frontwalldetected)
				position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.RightTurnOnly;
			else 
				position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.TRightStraight;
		}
		else if (!leftWallDetected && rightWallDetected) {
			// left beyond threshold (not detected) and right wall within threshold (detected)
			if (position.frontwalldetected)
				position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.LeftTurnOnly;
			else
				position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.TLeftStraight;
		}
		else if (!leftWallDetected && !rightWallDetected) {
			// left and right beyond threshold (not detected)
			
			// problem: if we detect opening on left and right straight away we can't detect in which lane are we currently in:
			// too close to left1, too close to left2, center, too close to right1, or too close to right2
			if (position.frontwalldetected)
				position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.TLeftRight;
			else
			    position.junctionLocation = m_prevPosition.junctionLocation = RobotJunctionLocation.FourWay;
		}
		else {
			assert(false);
		}
		
		double leftDistance = m_sensors.getLeftDistance();

		if (leftDistance >= LeftTooCloseDistance) {
			position.laneLocation = RobotLaneLocation.TooLeft;
		}
		else if (leftDistance >= LeftCloseDistance) {
			position.laneLocation = RobotLaneLocation.Left;
		}
		else {
			double rightDistance = m_sensors.getRightDistance();
			
			if (rightDistance >= RightTooCloseDistance) {
				position.laneLocation = RobotLaneLocation.TooRight;
			}
			else if (rightDistance >= RightCloseDistance) {
				position.laneLocation = RobotLaneLocation.Right;
			}
		}
		
		int[] frontDistances = m_sensors.getFrontDistances();

		for (int i = 0; i < 3; ++i) {
			if (frontDistances[i] >= FrontTooCloseDistance) {
				position.laneLocation |= RobotLaneLocation.FrontTooClose;
				break;
			}
		}
		
		if (!position.isFrontTooClose()) {
			for (int i = 0; i < 3; ++i) {
				if (frontDistances[i] >= FrontCloseDistance) {
					position.laneLocation |= RobotLaneLocation.FrontClose;
					break;
				}
			}
		}
		
		if (position.laneLocation == 0) {
			position.laneLocation = RobotLaneLocation.Center;
		}
		
		return position;
	}
	
	public boolean updateFrontWallStatus() throws ModbusException {
		return m_sensors.detectFrontWall();
	}
}
